{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "t0JD8EbaVWg-"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 2</h4>\n",
    "<font color=blue><h1>Nonlinear Dynamics (and Control) of an Inverted Pendulum System</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1is083NiFdHcHX8Hq56oh_AO35nQGO4bh)\n",
    "\n",
    "In this lecture we investigate the nonlinear dynamics of an inverted pendulum system.  More information on this example can be found in [FBS2e](https://fbswiki.org/wiki/index.php?title=FBS), Examples 3.3 and 5.4.  This lecture demonstrates how to use [python-control](https://python-control.org) to analyze nonlinear systems, including creating phase plane plots.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import the packages needed for the examples included in this notebook\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from math import pi\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "P_ZMCccjvHY1"
   },
   "source": [
    "## System model"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "Msad1ficHjtc"
   },
   "source": [
    "We consider an invereted pendulum, which is a simplified version of a balance system:\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/invpend-diagram.png\" alt=\"invpend.diagram\" width=100></center>\n",
    "\n",
    "The dynamics for an inverted pendulum system can be written as:\n",
    "\n",
    "$$\n",
    "  \\dfrac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\dot\\theta\\end{bmatrix} =\n",
    "    \\begin{bmatrix}\n",
    "      \\dot\\theta \\\\\n",
    "        \\dfrac{m g l}{J_\\text{t}} \\sin \\theta\n",
    "      - \\dfrac{b}{J_\\text{t}} \\dot\\theta\n",
    "      + \\dfrac{l}{J_\\text{t}} u \\cos\\theta\n",
    "    \\end{bmatrix}, \\qquad\n",
    "    y = \\theta,\n",
    "$$\n",
    "\n",
    "where $m$ and $J_t = J + m l^2$ are the mass and (total) moment of inertia of the system to be balanced, $l$ is the distance from the base to the center of mass of the balanced body, $b$ is the coefficient of rotational friction, and $g$ is the acceleration due to gravity.\n",
    "\n",
    "We begin by creating a nonlinear model of the system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "invpend_params = {'m': 1, 'l': 1, 'b': 0.5, 'g': 1}\n",
    "def invpend_update(t, x, u, params):\n",
    "    m, l, b, g = params['m'], params['l'], params['b'], params['g']\n",
    "    umax = params.get('umax', 1)\n",
    "    usat = np.clip(u[0], -umax, umax)\n",
    "    return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0] + usat/m)]\n",
    "invpend = ct.nlsys(\n",
    "    invpend_update, states=['theta', 'thdot'],\n",
    "    inputs=['tau'], outputs=['theta', 'thdot'],\n",
    "    params=invpend_params, name='invpend')\n",
    "print(invpend)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "IAoQAORFvLj1"
   },
   "source": [
    "## Open loop dynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "vOALp_IwjVxC"
   },
   "source": [
    "The open loop dynamics of the system can be visualized using the `phase_plane_plot` command in python-control:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.phase_plane_plot(\n",
    "    invpend, [-2*pi - 1, 2*pi + 1, -2, 2], 8),\n",
    "\n",
    "# Draw lines at the downward equilibrium angles\n",
    "plt.plot([-pi, -pi], [-2, 2], 'k--')\n",
    "plt.plot([pi, pi], [-2, 2], 'k--')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "WZuvqNzeJinm"
   },
   "source": [
    "We see that the vertical ($\\theta = 0$) equilibrium point is unstable, but the downward equlibrium points ($\\theta = \\pm \\pi$) are stable.\n",
    "\n",
    "Note also the *separatrices* for the equilibrium point, which gives insights into the regions of attraction (the red dashed line separates the two regions of attraction)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "2JibDTJBKHIF"
   },
   "source": [
    "## Proportional feedback\n",
    "\n",
    "We now stabilize the system using a simple proportional feedback controller:\n",
    "\n",
    "$$u = -k_\\text{p} \\theta.$$\n",
    "\n",
    "This controller can be designed as an input/output system that has no state dynamics, just a mapping from the inputs to the outputs:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set up the controller\n",
    "def propctrl_output(t, x, u, params):\n",
    "  kp = params.get('kp', 1)\n",
    "  return -kp * (u[0] - u[1])\n",
    "propctrl = ct.nlsys(\n",
    "    None, propctrl_output, name=\"p_ctrl\",\n",
    "    inputs=['theta', 'r'], outputs='tau'\n",
    ")\n",
    "print(propctrl)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "AvU35WoBMFjt"
   },
   "source": [
    "Note that the input to the controller is the reference value $r$ (which we will always take to be zero), the measured output $y$, which is the angle $\\theta$ for our system.  The output of the controller is the system input $u$, corresponding to the force applied to the wheels.\n",
    "\n",
    "To connect the controller to the system, we use the [`interconnect`](https://python-control.readthedocs.io/en/latest/generated/control.interconnect.html) function, which will connect all signals that have the same names:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the closed loop system\n",
    "clsys = ct.interconnect(\n",
    "    [invpend, propctrl], name='invpend w/ proportional feedback',\n",
    "    inputs=['r'], outputs=['theta', 'tau'], params={'kp': 1})\n",
    "print(clsys)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "IIiSaHNuM1u_"
   },
   "source": [
    "Note: you will see a warning when you run this command, because the output $\\dot\\theta$ (`thdot`) is not connected to anything.  You can ignore this here, but as you get to more complicated examples, you should pay attention to warnings of this sort and make sure they are OK."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can now linearize the closed loop system at different gains and compute the eigenvalues to check for stability:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Solution\n",
    "for kp in [0, 1, 10]:\n",
    "  print(\"kp = \", kp, \"; poles = \", clsys.linearize([0, 0], [0], params={'kp': kp}).poles())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "iV4u31DsNWP9"
   },
   "source": [
    "We see that at $k_\\text{p} = 10$ the eigenvalues (poles) of the closed loop system both have negative real part, and so the system is stabilized."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "Jg87a3iZP-Qd"
   },
   "source": [
    "### Phase portrait\n",
    "\n",
    "To study the resulting dynamics, we try plotting a phase plot using the same commands as before, but now for the closed loop system (with appropriate proportional gain):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.phase_plane_plot(\n",
    "    clsys, [-2*pi, 2*pi, -2, 2], 8, params={'kp': 10});"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "jhU2gidqi-ri"
   },
   "source": [
    "This plot is not very useful and has several errors.  It shows the limitations of the default parameter values for the `phase_plane_plot` command.\n",
    "\n",
    "Some things to notice in this plot:\n",
    "* Not all of the equilibrium points are showing up (there are two unstable equilibrium points that are missing)\n",
    "* There is no detail about what is happening near the origin."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Improved phase portrait\n",
    "\n",
    "To fix these issues, we can do a couple of things:\n",
    "* Restrict the range of the plot from $-3\\pi/2$ to $3\\pi/2$, which means that grid used to calculate the equilibrium point is a bit finer.\n",
    "* Reset the grid spacing, so that we have more initial conditions around the edge of the plot and a finer search for equilibrium points.\n",
    "\n",
    "Here's some improved code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "kp_params = {'kp': 10}\n",
    "ct.phase_plane_plot(\n",
    "    clsys, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n",
    "    gridspec=[13, 7], params=kp_params,\n",
    "    plot_separatrices={'timedata': 5})\n",
    "plt.plot([-pi, -pi], [-2, 2], 'k--', [ pi,  pi], [-2, 2], 'k--')\n",
    "plt.plot([-pi/2, -pi/2], [-2, 2], 'k:', [ pi/2,  pi/2], [-2, 2], 'k:');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Play around with some paramters to see what happens\n",
    "fig, axs = plt.subplots(2, 2)\n",
    "for i, kp in enumerate([3, 10]):\n",
    "  for j, umax in enumerate([0.2, 1]):\n",
    "    ct.phase_plane_plot(\n",
    "      clsys, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n",
    "      gridspec=[13, 7], plot_separatrices={'timedata': 5},\n",
    "      params={'kp': kp, 'umax': umax}, ax=axs[i, j])\n",
    "    axs[i, j].set_title(f\"{kp=}, {umax=}\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "dYeVbfG4kU-9"
   },
   "source": [
    "## State space controller\n",
    "\n",
    "For the proportional controller, we have limited control over the dynamics of the closed loop system.  For example, we see that the solutions near the origin are highly oscillatory in both the $k_\\text{p} = 3$ and $k_\\text{p} = 10$ cases.\n",
    "\n",
    "An alternative is to use \"full state feedback\", in which we set\n",
    "\n",
    "$$\n",
    "u = -K (x - x_\\text{d}) = -k_1 (\\theta - \\theta_d) - k_2 (\\dot\\theta - \\dot\\theta_d).\n",
    "$$\n",
    "\n",
    "We will learn more about how to design these controllers later, so if you aren't familiar with the idea of eigenvalue placement, just take this as a bit of \"control theory magic\" for now.\n",
    "\n",
    "To compute the gains, we make use of the `place` command, applied to the linearized system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Linearize the system\n",
    "P = invpend.linearize([0, 0], [0])\n",
    "\n",
    "# Place the closed loop eigenvalues (poles) at desired locations\n",
    "K = ct.place(P.A, P.B, [-1 + 0.1j, -1 - 0.1j])\n",
    "print(f\"{K=}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def statefbk_output(t, x, u, params):\n",
    "  K = params.get('K', np.array([0, 0]))\n",
    "  return -K @ (u[0:2] - u[2:])\n",
    "statefbk = ct.nlsys(\n",
    "    None, statefbk_output, name=\"k_ctrl\",\n",
    "    inputs=['theta', 'thdot', 'theta_d', 'thdot_d'], outputs='tau'\n",
    ")\n",
    "print(statefbk)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "clsys_sf = ct.interconnect(\n",
    "    [invpend, statefbk], name='invpend w/ state feedback',\n",
    "    inputs=['theta_d', 'thdot_d'], outputs=['theta', 'tau'], params={'kp': 1})\n",
    "print(clsys_sf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "aGm3usQIvmqN"
   },
   "source": [
    "### Phase portrait"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.phase_plane_plot(\n",
    "    clsys_sf, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n",
    "    gridspec=[13, 7], params={'K': K})\n",
    "plt.plot([-pi, -pi], [-2, 2], 'k--', [ pi,  pi], [-2, 2], 'k--')\n",
    "plt.plot([-pi/2, -pi/2], [-2, 2], 'k:', [ pi/2,  pi/2], [-2, 2], 'k:')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "A7UNUtfJwLWQ"
   },
   "source": [
    "Note that the closed loop response around the upright equilibrium point is much less oscillatory (consistent with where we placed the closed loop eigenvalues of the system dynamics)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "eVSa1Mvqycov"
   },
   "source": [
    "## Things to try\n",
    "\n",
    "Here are some things to try with the above code:\n",
    "* Try changing the locations of the closed loop eigenvalues in the `place` command\n",
    "* Try resetting the limits of the control action (`umax`)\n",
    "* Try leaving the state space controller fixed but changing the parameters of the system dynamics ($m$, $l$, $b$).  Does the controller still stabilize the system?\n",
    "* Plot the initial condition response of the system and see how to map time traces to phase plot traces."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
